/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "can.h"
#include "dma.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include "music.h"
#include "bmi088.h"
#include "IMU.h"
#include "PID.h"
#include "remote_control.h"
#include "motor_control.h"
#include "referee_system.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

uint32_t sys_time = 0; //系统时间 us
uint32_t cycle_num = 0; //主程序循环次数

float chip_temperature = 0.0f; //STM323芯片温度
float battery_voltage = 0.0f; //开发板供电电压

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/***********************************************************************
** 函 数 名： HAL_GPIO_EXTI_Callback()
** 函数说明： 外部中断回调函数
**---------------------------------------------------------------------
** 输入参数： 中断产生的外部GPIO引脚
** 返回参数： 无
***********************************************************************/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin == BMI088_GYRO_INT1_Pin)
	{
		BMI088_read_gyro(); //1000Hz
		
		AHRS_update(bmi088_gyro_calib, bmi088_accel); //IMU cal
	}
	else if(GPIO_Pin == BMI088_ACCEL_INT1_Pin)
	{
		BMI088_read_accel(); //800Hz
	}
	else if(GPIO_Pin == KEY_Pin)
	{
		//比赛时一定要注释掉
		gyro_zero_calib_flag = 1;
		//chassis_reset_id_flag = 1;
	}
}

/***********************************************************************
** 函 数 名： HAL_TIM_PeriodElapsedCallback()
** 函数说明： 定时器中断回调函数
**---------------------------------------------------------------------
** 输入参数： 中断产生的定时器
** 返回参数： 无
***********************************************************************/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim==&htim3) //500Hz 2ms
	{	
		if(robot_ctrl.movement_mode!=MOTOR_DISABLE)
		{
			set_chassis_motor_speed(robot_motor_speed.chassis_speed); //底盘电机闭环控速
			set_gimbal_ammobooster_motor_speed(robot_motor_speed.yaw_speed,robot_motor_speed.pitch_speed,robot_motor_speed.shoot_speed,robot_motor_speed.turnbullet_speed); //云台及发射机构电机闭环控速
		}
		else
		{
			CAN_cmd_chassis(0,0,0,0);
			CAN_cmd_gimbal_ammobooster(0,0,0,0,0);
			
			__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,400); //close shoot_wheel
			__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,400);
			
			HAL_GPIO_WritePin(LASER_GPIO_Port,LASER_Pin,GPIO_PIN_RESET); //close laser
		}
	}
	else if(htim==&htim6) //200Hz 5ms
	{
		if(robot_ctrl.movement_mode!=MOTOR_DISABLE)
		{
			robot_motion_resolving(&robot_ctrl,&robot_motor_speed); //运动解算
//			robot_motor_speed_control_test(); //速度环调试用
		}
	}
	else if(htim==&htim11) //100Hz 10ms
	{
		ParseRefereeSystemData(); //裁判系统解析
		
		//
		if(robot_referee_status.game_robot_status.robot_level != robot_referee_status.game_robot_status.robot_level_last)
		{
			if(robot_referee_status.game_robot_status.robot_level==1){robot_referee_status.shoot_data.bullet_speed_last=14;}
			else if(robot_referee_status.game_robot_status.robot_level==2){robot_referee_status.shoot_data.bullet_speed_last=17;}
			else if(robot_referee_status.game_robot_status.robot_level==3){robot_referee_status.shoot_data.bullet_speed_last=17;}
				
			robot_referee_status.game_robot_status.robot_level_last = robot_referee_status.game_robot_status.robot_level;
		}
		
		//
		if(robot_referee_status.shoot_data.recv_cnt != robot_referee_status.shoot_data.recv_cnt_last)
		{
			robot_ctrl.turnbullet_once_flag = 0;
			robot_referee_status.shoot_data.recv_cnt_last = robot_referee_status.shoot_data.recv_cnt;
			
			uint8_t tx_buf[5]={0};
			int16_t tmp = (0.3f*robot_referee_status.shoot_data.bullet_speed_last+0.7f*robot_referee_status.shoot_data.bullet_speed)*100; //0.01m/s
			tx_buf[0]=0x55; tx_buf[1]=0x12; tx_buf[2]=(tmp>>8); tx_buf[3]=tmp; tx_buf[4]=(tx_buf[1]+tx_buf[2]+tx_buf[3]);
			SendUART6DataPackage(tx_buf, 5);
			robot_referee_status.shoot_data.bullet_speed_last = robot_referee_status.shoot_data.bullet_speed;
		}
	}
	else if(htim==&htim7) //50Hz 20ms 
	{
		//播放音乐和提示音
		if(buzzer_state==PLAYING_INIT_MUSIC) {PlayingSong(song_robomasterlickdog,13);}
		else if(buzzer_state==PLAYING_WARNING_SOUND) {PlayingSound(sound_warning,6);}
		else if(buzzer_state==PLAYING_ERROR_SOUND) {PlayingSound(sound_error,6);}
		else if(buzzer_state==PLAYING_GYROCALIB_SOUND) {PlayingSound(sound_gyrocalibrating,6);}
		else if(buzzer_state==PLAYING_AUTOAIMING_SOUND) {PlayingSound(sound_autoaiming,6);}
	}
	else if(htim==&htim9) //2Hz 500ms
	{
		BMI088_read_temperature();
		uint16_t tmp = PIDIncControl(&pid_gyrotemp, bmi088_temperature);
		__HAL_TIM_SET_COMPARE(&htim10,TIM_CHANNEL_1,tmp);
		
		if(ABS(pid_gyrotemp.Error)<0.5f){HAL_GPIO_WritePin(LED_R_GPIO_Port,LED_R_Pin,GPIO_PIN_RESET); HAL_GPIO_TogglePin(LED_G_GPIO_Port,LED_G_Pin);}
		else{HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_RESET); HAL_GPIO_TogglePin(LED_R_GPIO_Port,LED_R_Pin);}
	}
	else if(htim == &htim12) // 10ms 计时
    {
        Timer12_10ms_Handler();
    }
}

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
	uint32_t rc_timeout_count=0;
	uint8_t i;
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
	memset((void *)(0x10000000),0,65536); //初始清零CCM数据RAM区
	IMU_init_data(); //初始化CCM区数据
	
  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_ADC1_Init();
  MX_ADC3_Init();
  MX_CAN1_Init();
  MX_CAN2_Init();
  MX_DMA_Init();
  MX_SPI1_Init();
  MX_TIM1_Init();
  MX_TIM10_Init();
  MX_TIM11_Init();
	MX_TIM12_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM5_Init();
  MX_TIM6_Init();
  MX_TIM7_Init();
  MX_TIM8_Init();
  MX_TIM9_Init();
  MX_USART1_UART_Init();
  MX_USART3_UART_Init();
  MX_USART6_UART_Init();
  /* USER CODE BEGIN 2 */
	
	CAN_filter_init_start();
	CAN_cmd_chassis(0,0,0,0);
	CAN_cmd_gimbal_ammobooster(0,0,0,0,0);
	
	HAL_TIM_Base_Start_IT(&htim11); //初始化裁判系统解析定时
	
	BMI088_init(); //初始化BMI088
	InitPID(); //初始化PID参数
	
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1); //shoot1_pwm
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2); //shoot2_pwm
	HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1); //bulletcover_pwm
	
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,400); //close shoot1_wheel
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,400); //close shoot2_wheel
	__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,BULLETROOM_COVER_CLOSE); //close bulletroom_cover
	
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3); //buzzer_pwm
	HAL_TIM_PWM_Start(&htim10,TIM_CHANNEL_1); //bmi088_temperature_pwm
	
	HAL_TIM_Base_Start_IT(&htim3); //初始化电机闭环控速定时
	HAL_TIM_Base_Start_IT(&htim6); //初始化运动解算定时
	HAL_TIM_Base_Start_IT(&htim7); //初始化音乐播放定时
	HAL_TIM_Base_Start_IT(&htim9); //初始化温度控制定时
	
	init_vrefint_coefficient(); //校准ADC基准电压
	
	SetBuzzerState(PLAYING_INIT_MUSIC); //播放启动音乐
	robot_ctrl.ui_show_mode = 0x82; RefreshClientUI(&robot_ctrl.ui_show_mode); //初始化客户端UI

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
	StartTimer2(); //主循环计时
	StartTimer5(); //自瞄数据接收间隔计时

	//
  while (1)
  {
		ClearTimer2();
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		//陀螺仪校准
		if(gyro_zero_calib_flag==1)
		{
			HAL_Delay(1000); //等待稳定
			SetBuzzerState(PLAYING_GYROCALIB_SOUND);
			BMI088_calib_gyro_zero(bmi088_gyro,bmi088_gyro_deg); //标定陀螺仪零点 ~10s
			gyro_zero_calib_flag = 0;
			SetBuzzerState(PLAYING_STOP);
		}
		
		//底盘电机ID快速重设
		if(chassis_reset_id_flag==1)
		{
			CAN_cmd_chassis_reset_ID();
			chassis_reset_id_flag=0;
		}
		
		//遥控器状态
		if(rc_update_flag==1)
		{
			rc_update_flag=0;
			
//			if(rc_timeout_count>=20){SetBuzzerState(PLAYING_STOP);}
//			rc_timeout_count=0;
		}
		else
		{
			rc_timeout_count++;
			if(rc_timeout_count>=20) //timeout-2s
			{
//				if(rc_timeout_count==20){if(SetBuzzerState(PLAYING_WARNING_SOUND)!=0){rc_timeout_count=0;}}
				robot_ctrl.movement_mode = MOTOR_DISABLE;
			}
		}
		
		//更新客户端UI刷新数据
		//
		client_ui_status.self_check_state = 0;
		
		client_ui_status.robot_id = robot_referee_status.game_robot_status.robot_id;
		client_ui_status.max_chassis_power = robot_referee_status.game_robot_status.chassis_power_limit;
		client_ui_status.shoot_speed_limit = robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit;
		client_ui_status.shoot_heat_limit = robot_referee_status.game_robot_status.shooter_id1_17mm_cooling_limit;
		client_ui_status.shoot_cooling_rate = robot_referee_status.game_robot_status.shooter_id1_17mm_cooling_rate;
		
		client_ui_status.battery_voltage = battery_voltage; if(client_ui_status.battery_voltage<21.0f){client_ui_status.self_check_state=2;}else if(client_ui_status.battery_voltage<22.0f){client_ui_status.self_check_state=1;}
		client_ui_status.bmi088_temperature = bmi088_temperature;
		client_ui_status.chip_temperature = chip_temperature;
		for(i=0;i<4;i++){client_ui_status.motor_temperature[i] = motor_status_chassis[i].temperature;}
		for(i=0;i<2;i++){client_ui_status.motor_temperature[4+i] = motor_status_gimbal[i].temperature;}
		for(i=0;i<3;i++){client_ui_status.motor_temperature[6+i] = motor_status_ammobooster[i].temperature;}
		
		if(autoaiming_ctrl.recv_interval==0){client_ui_status.auto_aiming_capture_fps=0.0f;}
		else{client_ui_status.auto_aiming_capture_fps=1000000.0f/autoaiming_ctrl.recv_interval;if(client_ui_status.auto_aiming_capture_fps<100.0f){client_ui_status.self_check_state=1;}}
		
		//
		client_ui_status.movement_mode = robot_ctrl.movement_mode;
		client_ui_status.chassis_lock_mode = robot_ctrl.chassis_lock_flag;
		client_ui_status.chassis_accelerate_mode = robot_ctrl.chassis_accelerate_flag;
		client_ui_status.ignore_shoot_heat_limit_mode = robot_ctrl.ignore_shoot_heat_limit_flag;
		client_ui_status.gimbal_autoaiming_control_mode = robot_ctrl.auto_aiming_flag;
		client_ui_status.bulletroom_open_flag = robot_ctrl.bulletroom_open_flag;
		
		client_ui_status.pitch_angle = -1*IMU_angle_deg[IMU_PITCH_INDEX];
		client_ui_status.yaw_delt_angle = -1*(YAW6020_FORWARD_POSITION-motor_status_gimbal[0].angle)*360.0f/8192;
		
		client_ui_status.supercap_voltage = supercap_voltage;
		client_ui_status.supercap_energy_percent = supercap_energy_percent;
		
		float tmp_speed[4]; for(i=0;i<4;i++){tmp_speed[i]=motor_status_chassis[i].speed_rpm/2.40494601831f;}
		float tmp_vx=(-tmp_speed[0]+tmp_speed[1]+tmp_speed[2]-tmp_speed[3])/4;
		float tmp_vy=(-tmp_speed[0]-tmp_speed[1]+tmp_speed[2]+tmp_speed[3])/4;
		float tmp_vw=(-tmp_speed[0]-tmp_speed[1]-tmp_speed[2]-tmp_speed[3])/(4*41.78371f);
		
		float tmp_out; arm_sqrt_f32(tmp_vx*tmp_vx+tmp_vy*tmp_vy,&tmp_out);
		client_ui_status.chassis_move_speed = tmp_out/3000; if(client_ui_status.chassis_move_speed>1.0f){client_ui_status.chassis_move_speed=1.0f;} //一格750mm/s
		client_ui_status.gyro_rotate_speed = ABS(tmp_vw)/60; if(client_ui_status.gyro_rotate_speed>1.0f){client_ui_status.gyro_rotate_speed=1.0f;} //一格15rpm
		for(i=0;i<2;i++){client_ui_status.shoot_motor_speed[i] = ABS(motor_status_ammobooster[i].speed_rpm)/8000.0f;} //一格2000rpm
		client_ui_status.turnbullet_frequency = -motor_status_ammobooster[2].speed_rpm/308.57143f; //一格5Hz
		
		client_ui_status.auto_aiming_num = autoaiming_ctrl.detect_state;
		if((autoaiming_ctrl.valid_count+autoaiming_ctrl.invalid_count)>0)
		{
			client_ui_status.auto_aiming_stability = 1.0f*autoaiming_ctrl.valid_count/(autoaiming_ctrl.valid_count+autoaiming_ctrl.invalid_count);
			autoaiming_ctrl.valid_count = 0;
			autoaiming_ctrl.invalid_count = 0;
		}
		else
		{
			client_ui_status.auto_aiming_stability = 0.0f;
		}

		//绘制客户端UI图形
		if(rc_ctrl.keyboard.key_bit.B)
		{
			uint8_t tx_buf[4]={0};
			if(robot_referee_status.game_robot_status.robot_id<100){tx_buf[0]=0x55; tx_buf[1]=0x01; tx_buf[2]=0x00; tx_buf[3]=(tx_buf[1]+tx_buf[2]);} //我红方
			else {tx_buf[0]=0x55; tx_buf[1]=0x01; tx_buf[2]=0x01; tx_buf[3]=(tx_buf[1]+tx_buf[2]);} //我蓝方
			SendUART6DataPackage(tx_buf, 4);
			
			robot_ctrl.ui_show_mode = robot_ctrl.ui_show_mode|0x80;
			RefreshClientUI(&robot_ctrl.ui_show_mode);
		}
		else
		{
			RefreshClientUI(&robot_ctrl.ui_show_mode);
		}
		
		//读取片内温度和电源电压
		if(cycle_num%10==0) //1Hz
		{
			chip_temperature = get_chip_temperature();
			battery_voltage = get_battery_voltage();
		}
		
		//
		while(ReadTimer2()<100000L); //100ms 10Hz
		sys_time+=ReadTimer2();
		cycle_num++;
		
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 6;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
